﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.IO;
using Mogre;

namespace WastedMechanics.NavMesh
{
    public class Mesh
    {
        const float VertexDistanceEpsilon = 0.1f;
        public List<Vertex> Vertices;
        public List<Edge> Edges;
        public List<Polygon> Polygons;

        PriorityQueue<float, Polygon> SearchQueue;

        public Mesh()
        {
            Vertices = new List<Vertex>();
            Edges = new List<Edge>();
            Polygons = new List<Polygon>();
            SearchQueue = new PriorityQueue<float, Polygon>();
        }

        public Vertex NewVertex(Vector3 pos)
        {
            Vertex match = Vertices.Find(vertex => (vertex.Position - pos).SquaredLength < VertexDistanceEpsilon);
            if (match != null)                            
                return match;            
            else
            {
                Vertex newVertex = new Vertex(pos);
                Vertices.Add(newVertex);
                return newVertex;
            }
        }

        public Edge NewEdge(Vertex beg, Vertex end)
        {
            Edge match = Edges.Find(edge => (edge.Begin == beg && edge.End == end) || (edge.Begin == end && edge.End == beg));
            if (match != null)                            
                return match;            
            else
            {
                Edge newEdge = new Edge(beg, end);
                Edges.Add(newEdge);
                return newEdge;
            }
        }

        public Polygon NewPolygon()
        {
            Polygon newPolygon = new Polygon(this);
            Polygons.Add(newPolygon);            
            return newPolygon;
        }

        public void ResetData()
        {
            Polygons.ForEach(polygon => polygon.ResetData()) ;
            SearchQueue.Clear();    
        }

        public Polygon FindClosestPolygon(Vector3 outsidePoint)
        {
            Polygon polygon = null;
            foreach (Polygon p in Polygons)
            {
                if (polygon == null)
                    polygon = p;
                else
                    if ((p.Center - outsidePoint).Length < (polygon.Center - outsidePoint).Length)
                        polygon = p;
            }
            return polygon;
        }
       
        public void SearchRoute(Vector3 start, Vector3 end, List<Vector3> points)
        {
            ResetData();
            points.Clear();

            Vector2 start2d = Helper.ProjXZ(start);
            Vector2 end2d = Helper.ProjXZ(end);
                       
            Polygon startPoly = null, endPoly = null;
            var matchPolys = Polygons.FindAll(poly => poly.IsPointInside(start2d));
            matchPolys.Sort((poly1, poly2) => (Mogre.Math.Abs(poly1.Center.y - start.y)).CompareTo(Mogre.Math.Abs(poly2.Center.y - start.y)));
            if (matchPolys.Count != 0)
            startPoly = matchPolys[0];
            matchPolys = Polygons.FindAll(poly => poly.IsPointInside(end2d));
            matchPolys.Sort((poly1, poly2) => (Mogre.Math.Abs(poly1.Center.y - end.y)).CompareTo(Mogre.Math.Abs(poly2.Center.y - end.y)));
            if (matchPolys.Count != 0)
                endPoly = matchPolys[0];

            if (startPoly != null && endPoly != null)
            {
                MarschrouteOptimizer marschroute = new MarschrouteOptimizer();
                marschroute.AddPolygon(startPoly);

                startPoly.CurrentCost = startPoly.FindCost(start, end);
                SearchQueue.Enqueue(startPoly.CurrentCost, startPoly);

                while (!SearchQueue.IsEmpty)
                {
                    Polygon first = SearchQueue.Dequeue();
                    first.IsVisited = true;                    

                    foreach (Edge edge in first.Edges)
                        foreach (Polygon neighbour in edge.Polygons)
                        {
                            if (neighbour.IsVisited)
                                continue;
                            float newCost = first.CurrentCost + (edge.Center - first.Center).Length + neighbour.FindCost(edge.Center, end);
                            if (neighbour.CurrentCost == 0 || newCost < neighbour.CurrentCost)
                            {
                                neighbour.CurrentCost = newCost;
                                SearchQueue.Enqueue(neighbour.CurrentCost, neighbour);
                            }
                        }

                    if (first == endPoly)
                        break;
                }

                if (endPoly.CurrentCost == 0)
                    return;

                List<Polygon> polys = new List<Polygon>();

                
                while (endPoly != startPoly)
                {
                    polys.Insert(0, endPoly);
                    foreach (Edge edge in endPoly.Edges)
                        foreach (Polygon neighbour in edge.Polygons)
                        {
                            if (neighbour == endPoly)
                                continue;
                            if (neighbour.CurrentCost < endPoly.CurrentCost)
                                endPoly = neighbour;
                        }
                }
                polys.ForEach(poly => marschroute.AddPolygon(poly));
                marschroute.GetPoints(start, end, points);
                points.Add(end);
            }
            else
                Console.WriteLine("OUPS! Someone got lost");
        }

        public void LoadFromFile(string meshfile)
        {
            using (BinaryReader reader = new BinaryReader(File.OpenRead(meshfile)))
            {
                int count = reader.ReadInt32();
                for (int i = 0; i < count; i++)
                {
                    Vector3 a = new Vector3(); ;
                    a.x = reader.ReadSingle();
                    a.y = reader.ReadSingle();
                    a.z = reader.ReadSingle();
                    Vector3 b = new Vector3(); ;
                    b.x = reader.ReadSingle();
                    b.y = reader.ReadSingle();
                    b.z = reader.ReadSingle();
                    Vector3 c = new Vector3(); ;
                    c.x = reader.ReadSingle();
                    c.y = reader.ReadSingle();
                    c.z = reader.ReadSingle();

                    NavMesh.Polygon poly = NewPolygon();
                    poly.BeginBuildingEdgeList();
                    poly.AddEdgeListVertex(a);
                    poly.AddEdgeListVertex(b);
                    poly.AddEdgeListVertex(c);                    
                    poly.EndBuildingEdgeList();
                    
                }
            }
        }

        public void Visualize()
        {
            foreach (Polygon poly in Polygons) poly.Visualize();            
        }

    }
}
